//
//  SurfMatch.h
//  BallCam_Program1
//
//  Created by 高大 堀田 on 12/07/05.
//
//      Edited by Cheng Li on 12/08/09
//
//  Copyright (c) 2012年 __MyCompanyName__. All rights reserved.
//

#ifndef SurfMatch_h
#define SurfMatch_h

#include <opencv2/opencv.hpp>
#include <opencv2/nonfree/features2d.hpp>
#include <opencv2/legacy/compat.hpp>			// legacy SURF code header

using namespace cv;
using namespace std;

//#define RATIO 0.6      //original 0.6 
//#define PROJ_ERROR 15  //original 15

class cmsurfMatch{

  public:
    double RATIO;
        double PROJ_ERROR;
    cmsurfMatch();
    ~cmsurfMatch();
    
    void computeDescriptorbySurf(Mat A, vector<KeyPoint> *keypoints0, vector<float> *descriptors0);
    void computeDescriptorbySift(Mat A, vector<KeyPoint> *keypoints0, Mat *descriptors0);
    void computeDescriptorbyOrb(Mat A, vector<KeyPoint> *keypoints0, Mat *descriptors0);
    void computeDescriptorbyBrief(Mat A, vector<KeyPoint> *keypoints0, Mat *descriptors0);

    Mat computeSurfMatch(Mat A, Mat B, int flag,int view,int feature); //view=0 Left-Right view=1 Top-Bottom

    Mat makeInvereseMap(Mat A, Mat H, Mat *K, Mat *D,int flag);

        Mat makeSuitInvereseMap(Mat & A, Mat H, Mat *K, Mat *D);

        void getInvereseSize(Mat & A, Mat H, int & dx, int & dy, int & width ,int & height);

        Mat makeSuitInvereseMap(Mat & A, Mat H, Mat *K, Mat *D,int dx,int dy,int width,int height);

        std::vector<Mat> computeSurfMatch(std::vector<Mat> A, Mat B, int flag, int view, int feature);

        Mat makeInvereseMap(std::vector<Mat> & A, std::vector<Mat> H, Mat *K, Mat *D,int flag,int & dx, int & dy,Mat & likelihood);

    void generateInverseMap(Size s,int dx,int dy,Mat &mapBtoAx,Mat &mapBtoAy,Mat Hba);
    
        void RobustEstimate(float * b,float * g,float * r,float * w,int n,float * color, float * std);

        Mat getLikelihood(Mat & A, Mat & H, Mat & mean, Mat & std,int dx,int dy);

        float get_p_value(Vec3b color, Vec3f mu,Vec3f std);

        

    enum {_Surf,_Sift,_ORB};
};



int surfimg_num=0;

cmsurfMatch::cmsurfMatch(){
    RATIO = 0.6;
        PROJ_ERROR = 15;
}

cmsurfMatch::~cmsurfMatch(){
    
}

void cmsurfMatch::computeDescriptorbySurf(Mat A , vector<KeyPoint> *keypoints0, vector<float> *descriptors0){
    Mat im0;
        cvtColor(A,im0,CV_BGR2GRAY);

    SURF surf_extractor(60,4,4,false);
    
    cout << "compute surfmatch...."<<endl;
    surf_extractor(im0, Mat::ones(im0.size(),CV_8U), *keypoints0, *descriptors0);

    cout << keypoints0->size() << endl;
    
    

    
}

void cmsurfMatch::computeDescriptorbySift(Mat A, vector<KeyPoint> *keypoints0, Mat *descriptors0){
    Mat im0;
        cvtColor(A,im0,CV_BGR2GRAY);
    
    
    SIFT sift_extractor(0,3,0.04,10,1.6);
    
    cout << "compute siftmatch...."<<endl;
    sift_extractor(im0, Mat::ones(im0.size(),CV_8U), *keypoints0, *descriptors0);
    cout << keypoints0->size() << endl;

    
}

void cmsurfMatch::computeDescriptorbyOrb(Mat A, vector<KeyPoint> *keypoints0, Mat *descriptors0){
    Mat im0;
        cvtColor(A,im0,CV_BGR2GRAY);
    
  
    ORB orb_extractor;
    
    cout << "compute ORB match...."<<endl;
    orb_extractor(im0, Mat::ones(im0.size(),CV_8U), *keypoints0, *descriptors0);
    cout << keypoints0->size() << endl;
    
}

void cmsurfMatch::computeDescriptorbyBrief(Mat A, vector<KeyPoint> *keypoints0, Mat *descriptors0){
    Mat im0;
        cvtColor(A,im0,CV_BGR2GRAY);
    
    cout << "compute Brief match...."<<endl;

    cv::SiftFeatureDetector detector(0.05,10.0);
    detector.detect(im0, *keypoints0);

    cv::BriefDescriptorExtractor extractor;
    extractor.compute(im0, *keypoints0, *descriptors0);

    
        
}

Mat cmsurfMatch::computeSurfMatch(Mat A, Mat B, int flag, int view,int feature){
    //Mat im0,im1;
        //cvtColor(A,im0,CV_BGR2GRAY);
        //cvtColor(B,im1,CV_BGR2GRAY);
        cv::vector<KeyPoint> keypoints0; cv::vector<float> descriptors0;
    cv::vector<KeyPoint> keypoints1; cv::vector<float> descriptors1;
    
    cv::Mat descriptors0_mat;
        cv::Mat descriptors1_mat;
    
    if(feature == _Surf){
        computeDescriptorbySurf(A, &keypoints0, &descriptors0);
        computeDescriptorbySurf(B, &keypoints1, &descriptors1);
        Mat surf_descriptors0(descriptors0);
        Mat surf_descriptors1(descriptors1);
        
        surf_descriptors0.copyTo(descriptors0_mat);
        surf_descriptors1.copyTo(descriptors1_mat);
        
        
    }
    else if(feature == _Sift){
        Mat siftdescriptors0,siftdescriptors1;
        computeDescriptorbySift(A, &keypoints0, &siftdescriptors0);
        computeDescriptorbySift(B, &keypoints1, &siftdescriptors1);
        
        siftdescriptors0.copyTo(descriptors0_mat);
        siftdescriptors1.copyTo(descriptors1_mat);
    }
    else if(feature == 3){
        Mat orbdescriptors0,orbdescriptors1;
        computeDescriptorbyOrb(A, &keypoints0, &orbdescriptors0);
        computeDescriptorbyOrb(B, &keypoints1, &orbdescriptors1);
        
        orbdescriptors0.copyTo(descriptors0_mat);
        orbdescriptors1.copyTo(descriptors1_mat);
        
    }
    else if(feature == 4){
        Mat briefdescriptors0,briefdescriptors1;
        computeDescriptorbyOrb(A, &keypoints0, &briefdescriptors0);
        computeDescriptorbyOrb(B, &keypoints1, &briefdescriptors1);
        
        briefdescriptors0.copyTo(descriptors0_mat);
        briefdescriptors1.copyTo(descriptors1_mat);
    }
    
    //cout << keypoints0.size()<< endl;
    //cout << keypoints1.size()<< endl;
    
    
    //descriptor_matまで出してやれば後は一緒。つまりここまで関数化してやればいいと思う
    descriptors0_mat = descriptors0_mat.reshape(1,keypoints0.size());
    descriptors1_mat = descriptors1_mat.reshape(1,keypoints1.size());
        cv::flann::Index flann_index(descriptors1_mat, cv::flann::KDTreeIndexParams(4));
    //cv::flann::Index flann_index(descriptors1_mat, cv::flann::KMeansIndexParams(4));

    cv::Mat inds(keypoints0.size(),2, CV_32S);
    cv::Mat dists(keypoints0.size(),2, CV_32F);
        flann_index.knnSearch(descriptors0_mat, inds, dists, 2, cv::flann::SearchParams(64));
    cv::vector<cv::Point2f> im0_points, im1_points;

        int * used = new(int[keypoints1.size()]);
        for (int i = 0; i < (int)keypoints1.size(); ++i) used[i]=0;
        
        for (int i = 0; i < (int)keypoints0.size(); ++i){
                float d0 = dists.at<float>(i,0);
            float d1 = dists.at<float>(i,1);
        
        if (d0 < RATIO*d1){ // sufficiently far (60%) from second nearest
                        cv::KeyPoint im0_point = keypoints0.at(i);
                        int val = inds.at<int>(i,0);
            cv::KeyPoint im1_point = keypoints1.at(val);
                        if(used[val]==0){ // avoid multiple assignments to same point
                                im0_points.push_back(im0_point.pt);
                                im1_points.push_back(im1_point.pt);
                                used[val]=1;
                        }
        }
    }
        
        Mat H = Mat::eye(3,3,CV_64FC1);
        
        if(im0_points.size()>6){
                //cout << "Find Homography" << endl;
                cv::vector<uchar> status;


                if(flag == 2){
                        H  = findHomography(Mat(im0_points),Mat(im1_points),status,CV_RANSAC,3);
                        cout << flag << endl;
                }
                else H  = findHomography(Mat(im0_points),Mat(im1_points),status,CV_RANSAC,PROJ_ERROR);

                if(1)
                {
                        
                        H = Mat::eye(3,3,CV_64FC1);
                        //system("pause");
                        //H = getAffineTransform(Mat(im0_points),Mat(im1_points));
                        Mat M = estimateRigidTransform( im0_points , im1_points, false);
                        M.copyTo(H.rowRange(0,2));

                        
                }
                // draw point correspondences...
                
                Mat dsp;
                
        if(view==0){
        // LEFT-RIGHT
        dsp = Mat(A.rows,A.cols*2,CV_8UC3);
        Mat left = Mat(dsp,Rect(0,0,A.cols,A.rows));
        A.copyTo(left);
        Mat right = Mat(dsp,Rect(A.cols,0,B.cols,B.rows));
        B.copyTo(right);
        }else if(view == 1){
                // TOP-BOTTOM           
                dsp = Mat(A.rows*2,A.cols,CV_8UC3);
                Mat top = Mat(dsp,Rect(0,0,A.cols,A.rows));
                A.copyTo(top);
                Mat bot = Mat(dsp,Rect(0,A.rows,B.cols,B.rows));
                B.copyTo(bot);
                }
                
                //cout << "Draw coorespondences" << endl;
                
                for(int i=0;i<(int)im0_points.size();i++){
                        if(status[i]==1){
                                circle(dsp, im0_points[i], 2, CV_RGB(255,255,0),-1,CV_AA);
                                
                if(view == 0){
                // LEFT-RIGHT
                                circle(dsp, Point(im1_points[i].x+A.cols,im1_points[i].y),2, CV_RGB(255,255,0),-1,CV_AA);
                                line(dsp, im0_points[i], Point(im1_points[i].x+A.cols,im1_points[i].y), CV_RGB(0,255,0),1,CV_AA);
                                }else if(view == 1){
                                //TOP-BOTTOM
                                circle(dsp, Point(im1_points[i].x,im1_points[i].y+A.rows),2, CV_RGB(255,255,0),-1,CV_AA);
                                line(dsp, im0_points[i], Point(im1_points[i].x,im1_points[i].y+A.rows), CV_RGB(0,255,0),1,CV_AA);
                                }
                                //float dy = im0_points[i].y - im1_points[i].y;
                                //float dx = im0_points[i].x - im1_points[i].x;
                                //cout << "dx=" << dx << ", dy=" << dy << endl;
                        }
                }
                
                stringstream ss;
                
                ss << flag;
                //imshow(ss.str(), dsp); 
        // stringstream sss;
        char CutImg[100];
        sprintf(CutImg,"/Users/Kodai/Documents/Ball_Project/BallCam_Program1/BallCam_Program1/Result/surf%04d.jpg",surfimg_num);
        imwrite(CutImg,dsp);
        surfimg_num++;
                //waitKey(1);
                
                
                //Mat sm;
                //resize(dsp,sm,Size(A.rows,A.cols),0,0,INTER_NEAREST);
                //resize(dsp,sm,Size(A.cols,A.rows),0,0,INTER_NEAREST);
        }
        //else cout << "***** Not enough points" << endl;
        
        
        //cout << "compute SURF match DONE." <<endl;
        Mat_<double> h = H;     
        
        for(int i=0;i<3;i++){ 
        for(int j=0;j<3;j++){
                        cout << h(i,j) << " ";
        }
        }
        cout << endl;
    
        
        //float rat =  h(0,0) / h(1,1);
        
        //cout <<endl << rat << endl;
        
        /*
     if( (rat < 0.3) || (rat>3.0) || fabs(h(0,2))<1){
     cout << "**** INVALID HOMOGRAPHY ***" << endl;
     H = Mat::eye(3,3,CV_64FC1);
     }
         */
        
    cout<< "finish."<<endl;
        
        return H;

}

std::vector<Mat> cmsurfMatch::computeSurfMatch(std::vector<Mat> A, Mat B, int flag, int view, int feature)
{
        std::vector<Mat> ans;
        for(int i = 0;i<(int)A.size();i++)
        {
                Mat H = computeSurfMatch(A[i],B,flag,view,feature);
                ans.push_back( H );
        }
        return ans;
}


void cmsurfMatch::generateInverseMap(Size s,int dx,int dy,Mat &mapBtoAx,Mat &mapBtoAy,Mat Hba){
    //cout << "generateInverseMap..."<<endl;
        Mat Ix,Iy;
        initUndistortRectifyMap(Mat::eye(3,3,CV_64F), Mat::zeros(5,1,CV_64F),Mat::eye(3,3,CV_64F), Mat::eye(3,3,CV_64F),s, CV_32FC1,Ix,Iy);
        Ix -= dx; // minus right shift
        Iy -= dy; // minus downshift
        Mat_<float> ix = Ix;
        Mat_<float> iy = Iy;
        mapBtoAx = Ix * 1.0; // copy
        mapBtoAy = Iy * 1.0; // copy
        Mat_<float> h = Hba.inv(DECOMP_LU);
        Mat_<float> x = mapBtoAx;
        Mat_<float> y = mapBtoAy;
        for(int r=0;r< s.height;r++){
                for(int c=0;c<s.width;c++){ // x
                        float z =  h(2,0)*ix(r,c) + h(2,1)*iy(r,c) + h(2,2);       // normalization constant
                        x(r,c) = ( h(0,0)*ix(r,c) + h(0,1)*iy(r,c) + h(0,2) ) / z + dx; // homography warp (on coordinates)
                        y(r,c) = ( h(1,0)*ix(r,c) + h(1,1)*iy(r,c) + h(1,2) ) / z + dy; // hmography warp (on coordinates)
                }
        }
}


Mat cmsurfMatch::makeSuitInvereseMap(Mat & A, Mat H, Mat *K, Mat *D,int dx,int dy,int width,int height)
{
        Mat Big = Mat::zeros(height,width,A.type());

        Size s = Big.size();
        
        Mat mapA2Ax,mapA2Ay;

        generateInverseMap(s,dx,dy,mapA2Ax,mapA2Ay,H); // Map A to base A

        Mat M;

        Big*=0.0;
        M =  Mat(Big,Rect(dx,dy,A.cols,A.rows));
        A.copyTo(M);
        Mat BigA = Big*0.0;
        if(Big.type()==0) remap(Big,BigA,mapA2Ax,mapA2Ay,INTER_NEAREST,BORDER_CONSTANT);
        else remap(Big,BigA,mapA2Ax,mapA2Ay,INTER_CUBIC,BORDER_CONSTANT);

        

        if(0)
        {
                imshow("debug Big" , Big);
                imshow("debug BigA", BigA);

                cv::waitKey(0);
        }
        //Mat invH = H.inv();
        //generateInverseMap(s,dx,dy,mapA2Ax,mapA2Ay,H); // Map A to base A

    
        //cout << BigA.rows << " == " << height << "  " << BigA.cols << " == " << width << endl;
    return BigA;
}

void cmsurfMatch::getInvereseSize(Mat & A, Mat H, int & dx, int & dy, int & width ,int & height)
{
        double v[4][3] = {{0,0,1},{A.cols,0,1},{0,A.rows,1},{A.cols,A.rows,1}};

        double minx=0;
        double maxx=A.cols;
        double miny=0;
        double maxy=A.rows;

        for(int i = 0;i<4;i++)
        {
                Mat m_v(3,1,CV_64F,v[i]);
                m_v = H*m_v;
                m_v = m_v/m_v.at<double>(2);

                if(m_v.at<double>(0) < minx) minx = m_v.at<double>(0);
                else if(m_v.at<double>(0) > maxx) maxx = m_v.at<double>(0);
                
                if(m_v.at<double>(1) < miny) miny = m_v.at<double>(1);
                else if(m_v.at<double>(1) > maxy) maxy = m_v.at<double>(1);
        }
        minx = floor(minx);
        miny = floor(miny);

        maxx = floor(maxx);
        maxy = floor(maxy);

        //cout << "[ " << minx << " " << maxx  << " ] [ " << miny << " " << maxy << " ] " << endl;

        dx = (int)-minx;
        dy = (int)-miny;

        width = (int)(maxx-minx);
        height = (int)(maxy-miny);
}

Mat cmsurfMatch::makeSuitInvereseMap(Mat & A, Mat H, Mat *K, Mat *D)
{
        

        int dx;
        int dy;
        int width;
        int height;
        getInvereseSize(A,H,dx,dy,width,height);
        return makeSuitInvereseMap(A,H,K,D,dx,dy,width,height);
}


Mat cmsurfMatch::makeInvereseMap(Mat A, Mat H, Mat *K, Mat *D,int flag){
    
    Mat Big = Mat::zeros(A.rows*2,A.cols*2,A.type());
        
        int dx,dy;
        
    if(flag<10){
                Big = Mat::zeros(A.rows*2,A.cols*2,A.type());
                dx= round(Big.cols*0.25);
                dy = round(Big.rows*0.25);
        }
        else{
                cout << "no expansion " << flag << endl;
                Big = Mat::zeros(A.rows,A.cols,A.type());       // do not expand canvas
                dx = 0;
                dy = 0;
        }
        
        if( (flag%10) == 1){
                cout << "whiteout " << (flag%10) << endl;
                Big = 255;
        }

        Size s = Big.size();
        
        Mat mapA2Ax,mapA2Ay;
        generateInverseMap(s,dx,dy,mapA2Ax,mapA2Ay,H); // Map A to base A
        
        Mat M;
        
        // when centered with border, zero out offset
        //dx=0;
        //dy=0;
        //Mat BigB,BigC;        
    
        
        Big*=0.0;
        M =  Mat(Big,Rect(dx,dy,A.cols,A.rows));
        A.copyTo(M);
        Mat BigA = Big*0.0;
        if(Big.type()==0) remap(Big,BigA,mapA2Ax,mapA2Ay,INTER_NEAREST,BORDER_CONSTANT);
        else remap(Big,BigA,mapA2Ax,mapA2Ay,INTER_CUBIC,BORDER_CONSTANT);

        
    
        
    return BigA;
}


void cmsurfMatch::RobustEstimate(float * b,float * g,float * r,float * w,int n,float * color,float * std)
{
        int count = 0;
        for(int i = 0;i<n;i++) if(w[i]>0.1) count++;

        float * nb = new(float[count]);
        float * ng = new(float[count]);
        float * nr = new(float[count]);

        count = 0;
        for(int i = 0;i<n;i++) if(w[i]>0.1)
                {nb[count] = b[i]; ng[count] = g[i]; nr[count] = r[i]; count++; }

        sort(nb,nb+count); sort(ng,ng+count); sort(nr,nr+count);

        int num_out = floor(count*0.15);

        if(count <=2) num_out = 0;
        else if(num_out==0) num_out = 1;
        

        int left = num_out;
        int right = count-num_out;


        color[0] = 0; color[1] = 0; color[2] = 0;
        std[0] = 0; std[1] = 0; std[2] = 0;

        for(int i = left;i<right;i++)
        {
                color[0] += nb[i];color[1] += ng[i];color[2] += nr[i];
        }

        color[0]/=(right-left); color[1]/=(right-left); color[2]/=(right-left);

        for(int i = left;i<right;i++)
        {
                std[0] += (nb[i]-color[0])*(nb[i]-color[0]);
                std[1] += (ng[i]-color[1])*(ng[i]-color[1]);
                std[2] += (nr[i]-color[2])*(nr[i]-color[2]);
        }

        std[0]/=(right-left); std[1]/=(right-left); std[2]/=(right-left);
        std[0] = sqrt(std[0]); std[1] = sqrt(std[1]); std[2] = sqrt(std[2]);

        if(right-left==1)
        {
                std[0]= 40; std[1]= 40; std[2]= 40;
        }
        else
        {
                std[0]+= 20; std[1]+= 20; std[2]+= 20;
        }

		delete[] nb;
		delete[] ng;
		delete[] nr;
        
}

float cmsurfMatch::get_p_value(Vec3b color, Vec3f mu,Vec3f std)
{
        float ans = 1;
        for(int k = 0;k<3;k++)
        {
                float wid = std[k]*5;
                float dif = wid-fabs(color[k] - mu[k]);

                if(dif<0) ans *= 1e-3;
                else ans*=(dif*dif/wid/wid+1e-3);
        }
        return ans;
}


Mat cmsurfMatch::getLikelihood(Mat & A, Mat & H, Mat & mean, Mat & std,int dx,int dy)
{
        

        Mat likelihood = Mat::zeros(A.size(),5);

        int maxw = mean.cols;
        int maxh = mean.rows;

        Mat debug_mean = Mat::zeros(A.size(),CV_32FC3);
        Mat debug_std = Mat::zeros(A.size(),CV_32FC3);
        Mat debug_now = Mat::zeros(A.size(),CV_32FC3);

        for(int y = 0;y< A.rows;y++)
                for(int x = 0; x<A.cols;x++)
                {
                        double pos[] = {x,y,1};

                        Mat m_v(3,1,CV_64F,pos);
                        m_v = H*m_v;
                        m_v = m_v/m_v.at<double>(2);

                        m_v.convertTo(m_v,CV_32S);
                        
                        int u = m_v.at<int>(0)+dx;
                        int v = m_v.at<int>(1)+dy;//row

                        if(u<0) u = 0;
                        if(v<0) v = 0;

                        if(u>maxw-2) u = maxw-1;
                        if(v>maxh-2) v = maxh-1;//row

                        //float maxli = 0;

                        int l = u;
                        int k = v;
                        Vec3f m_mean = mean.at<Vec3f>(k,l);
                        Vec3f m_std = std.at<Vec3f>(k,l);

                                        
                        debug_mean.at<Vec3f>(y,x) = m_mean;
                        debug_std.at<Vec3f>(y,x) = m_std;


                }

        

        int win_size = 2;

        int patch_sz = (2*win_size+1) * (2*win_size+1);

        for(int y = win_size;y< debug_mean.rows-win_size;y++)
                for(int x = win_size; x<debug_mean.cols-win_size;x++)
                {
                        Vec3f std = 0;
                        Vec3f mean = 0;
                        for(int dx = -win_size;dx<win_size+1;dx++)
                                for(int dy = -win_size;dy<win_size+1;dy++)
                                {
                                        Vec3f temp = debug_mean.at<Vec3f>(y+dy,x+dx);
                                        Vec3f temp_std = debug_std.at<Vec3f>(y+dy,x+dx);
                                        mean = mean+ temp;
                                        for(int k = 0;k<3;k++)
                                        {
                                                std[k] = std[k]+temp[k]* temp[k] + temp_std[k]*temp_std[k];
                                        }
                                }

                        
                        mean = mean/patch_sz;

                        std = std/patch_sz;

                        
                        for(int k = 0;k<3;k++){
                                std[k]-=mean[k]*mean[k];
                                std[k] = sqrt( std[k] );
                        }
                        
                        likelihood.at<float>(y,x) = get_p_value(A.at<Vec3b>(y,x),mean,std);
                }

        for(int dx = -win_size;dx<win_size+1;dx++)
                for(int dy = -win_size;dy<win_size+1;dy++)
                {
                        if(dx==0 && dy==0) continue;

                        for(int y = win_size +dy ;y< debug_mean.rows-win_size +dy;y++)
                                for(int x = win_size +dx; x<debug_mean.cols-win_size +dx;x++)
                                {
                                        Vec3f mean = debug_mean.at<Vec3f>(y-dy,x-dx);
                                        Vec3f std = debug_std.at<Vec3f>(y-dy,x-dx);
                                        likelihood.at<float>(y,x) = max(likelihood.at<float>(y,x), get_p_value(A.at<Vec3b>(y,x),mean,std));
                                }


                }

        return likelihood;
}


Mat cmsurfMatch::makeInvereseMap(std::vector<Mat>  & A, std::vector<Mat> H, Mat *K, Mat *D,int flag,int & dx, int & dy,Mat & likelihood)
{
        if(A.size()<1) return Mat();

        int minx = 0;
        int miny = 0;
        int maxx = 0;
        int maxy = 0;

        for(int i = 0;i<(int)H.size();i++)
        {
                int dx,dy,width,height;
                getInvereseSize(A[i],H[i],dx,dy,width,height);

				if(width/height>10 || height/width>10 || height/A[0].rows>10 || width/A[i].cols>10)
				{
					continue;
				}
				else
				{
					if(-dx<minx) minx = -dx;
					if(-dy<miny) miny = -dy;
					if(-dx+width>maxx) maxx = -dx+width;
					if(-dy+height>maxy) maxy = -dy+height;
				}
        }


		
        dx = -minx;
        dy = -miny;
        

        //cout << "bigsize " << "[ " << minx << " " << maxx  << " ] [ " << miny << " " << maxy << " ] " << endl;

        std::vector<Mat> vec_bigimg;
        std::vector<Mat> vec_weightimg;

		


        for(int i = 0;i<(int)H.size();i++)
        {

                Mat Big = Mat::zeros(maxy-miny,maxx-minx,A[i].type());

                int dx,dy,width,height;
                getInvereseSize(A[i],H[i],dx,dy,width,height);

				if(-dy-miny<0) dy = -miny;
				if(-dx-minx<0) dx = -minx;


                Mat E = makeSuitInvereseMap(A[i],H[i],K,D,dx,dy,width,height);

                //imshow("debug",E);cv::waitKey(10);

                E.copyTo(Big(Range(-dy-miny,-dy-miny+height),Range(-dx-minx,-dx-minx+width)));

                cv::GaussianBlur(Big,Big,Size(7,7),1.5,1.5);

                vec_bigimg.push_back(Big);

                Mat WightImg = Mat::zeros(maxy-miny,maxx-minx,CV_8U);
                
                Mat onesimg = Mat::ones(A[i].size(),CV_8U)*100;
                E = makeSuitInvereseMap(onesimg , H[i],K,D,dx,dy,width,height);
                E.copyTo(WightImg(Range(-dy-miny,-dy-miny+height),Range(-dx-minx,-dx-minx+width)));

                //imshow("debug weight",E);cv::waitKey(10);

                vec_weightimg.push_back(WightImg);
				
        }
		

        Mat mean_img = Mat::zeros(maxy-miny,maxx-minx,CV_32FC3);

        Mat std_img = Mat::zeros(maxy-miny,maxx-minx,CV_32FC3);

		

        if(1)
        {

			float * b = new(float[H.size()]);
            float * g = new(float[H.size()]);
            float * r = new(float[H.size()]);
            float * w = new(float[H.size()]);

			float * color = new(float[3]);
            float * std = new(float[3]);

                for(int y = 0; y < mean_img.rows; y++)
                        for(int x = 0; x < mean_img.cols; x++)
                        {
                                
                                for(int i = 0;i<(int)H.size();i++)
                                {
                                        cv::Vec3b tempcolor = vec_bigimg[i].at<Vec3b>(y,x);
                                        b[i] = tempcolor[0]; g[i] = tempcolor[1]; r[i] = tempcolor[2];
                                        w[i] = (float)vec_weightimg[i].at<unsigned char>(y,x);                          
                                }
                                
                                RobustEstimate(b,g,r,w,H.size(),color,std);

                                mean_img.at<Vec3f>(y,x) = Vec3f(color[0],color[1],color[2]);

                                //mean_img.at<Vec3f>(y,x) = Vec3f(std[0],std[1],std[2]);
                                std_img.at<Vec3f>(y,x) = Vec3f(std[0],std[1],std[2]);
                        }


                        

                        Mat L = getLikelihood(A[A.size()-2],H[A.size()-2],mean_img,std_img,dx,dy);

                        //cv::destroyAllWindows();

                        L = L;
                        L = L*255;

                        L.convertTo(L,CV_8U);

                        L.copyTo(likelihood);

                        L.release();


                        //imshow("likelihood",L);

                        //imwrite("likelihood.jpg",L);
        }
        else
        {
                

                for(int i = 0;i<(int)H.size();i++)
                {
                        vec_bigimg[i].convertTo(vec_bigimg[i],CV_32FC3);

                        mean_img = mean_img + vec_bigimg[i];

                }
                mean_img = mean_img/H.size();

                
        }

        

        mean_img.convertTo(mean_img,CV_8UC3);


        

		for(int i = 0;i<(int)vec_bigimg.size();i++)
		{
			vec_bigimg[i].release();
			vec_weightimg[i].release();
			
		}

        return mean_img;


}


#endif
